#!/usr/bin/python
# -*- coding: utf-8 -*-

import rospy
from tf.msg import tfMessage
from velodyne_msgs.msg import VelodyneScan, VelodyneScanUnified
from sensor_msgs.msg import Imu
import std_msgs
import time
import rosbag
from std_msgs.msg import Clock

filename = '/media/bobin/Seagate/data/baidu/cyberecord_KL038_default_23_20180822140620_20180822140720.bag'

def main():
  rospy.init_node('convert_msgs')
  publisher = rospy.Publisher('/velodyne_packets', VelodyneScan, queue_size=1)
  pub_imu = rospy.Publisher('/imu', Imu, queue_size=1)
  pub_clk = rospy.Publisher('/clock', Clock, queue_size=1)

  with rosbag.Bag(filename, 'r') as bag:
    for topic, _msg, t in bag.read_messages():
      if topic == "/sensor/novatel/Imu":
        try:
          h = std_msgs.msg.Header()
          h.stamp = t
          print "imu", t
          _msg.header = h
          pub_imu.publish(_msg)
        except e:
          print e
      if topic == "/sensor/velodyne16/left/VelodyneScan":
        try:
          print "points", _msg.header
          scan = VelodyneScan()
          scan.header = _msg.header
          scan.packets = _msg.packets
          publisher.publish(scan)
        except e:
          print e
      if topic == "/clock":
        pub_clk.publish(_msg)
      rospy.sleep(0.01)

if __name__ == '__main__':
  main()